#include "loc_correction/real_time_outlier_removal.h"

double RealTimeOutlierRemoval::process(double new_value) {
    if (!std::isfinite(new_value)){
        return last_valid_value_;
    }
    count_++;
    double delta = new_value - mean_;
    mean_ += delta / count_;
    stdev_ += delta * (new_value - mean_);

    double current_stdev = getStdDev();
    double threshold_value = threshold_ * current_stdev;

    if (std::abs(new_value - mean_) > threshold_value){
        return last_valid_value_;
    }else{
        last_valid_value_ = new_value;
        return new_value;
    }
}